from machine import Pin
import sensor, image
import  pyb
from pyb import UART, LED,Pin, Timer
import  ustruct
import time
import math

red_td1 =[(12, 100, 14, 69, -23, 28)] #自然光
red_td =[(12, 100, 14, 69, -23, 28)]
#red_td =[(58, 98, 32, 79, 54, -15)]
green_td1 =[(8, 100, -108, -18, -37, 77)]
green_td=[(8, 100, -108, -20, -49, 80)]
red_led   = LED(1)
offset1=4
x120=142-offset1
x40=22+offset1
y100=118-offset1
y20=2+offset1
add1=0
add2=0
array_posx=[80,x40,x40,x40, 60     , 80     ,100     ,x120,x120,x120+add1,x120+add2,x120+add1,x120,x120,x120, 100     ,  80     ,  60     , x40, x40, x40,x40-add1,x40-add2,x40-add1,x40,x40,x40,80,80]
array_posy=[60,y20,y20,y20,y20+add1,y20+add2,y20+add1, y20, y20,  40     ,  60     ,  80     ,y100,y100,y100,y100-add1,y100-add2,y100-add1,y100,y100,y100, 80     , 60     , 40     ,y20,y20,y20,60,60]

roi1=(x40*2,y20*2,(x120-x40)*2,(y100-y20)*2)

bx=0
by=0
max_array=26

count_point=0
start=1
mode =0

temp_count=0
tempy=0
tempx=0
mode_last=0

uart = UART(3, 115200) # todo: 几号口不确定
def sending_data(cx1, cy1):
    global uart
    #print(cx1,cy1)
    cx1=cx1+320
    cy1=cy1+240
    data = ustruct.pack("<bhhb",  # 格式为一个字符和两个短整型(2字节)
                        0x86,   # 帧头1
                        cx1 ,    # 数据1
                        cy1,
                        0x54)    # 数据2
    uart.write(data )   # 在数据后面添加额外的字节 0x54

start1=1
def tick(timer):#将每一段再进行细分
    global start1, temp_count, count_point, max_array, offset_x, offset_y,bx,by,tempx,tempy
    divide_count=15
    if  mode ==1 and mode!=7:
        if start1==1:
            if temp_count==divide_count:
                temp_count=0
                count_point+=1
                if count_point == max_array:
                    start1=0
            else :
                temp_count+=1
        tempx=bx-2*(array_posx[count_point]+((array_posx[count_point+1]-array_posx[count_point])*temp_count/divide_count))
        tempy=by-2*(array_posy[count_point]+((array_posy[count_point+1]-array_posy[count_point])*temp_count/divide_count))
        #print(tempx,tempy)
#timer=pyb.Timer(2,freq=25)
#timer.callback(tick)
#timer.init()
tim = Timer(2, freq=25)      # 使用定时器2创建定时器对象-以1Hz触发
tim.callback(tick)          # 将回调设置为tick函数
def sensor_init(a):
    # 初始化摄像头
    if a==1:
        sensor.reset()
        sensor.set_pixformat(sensor.RGB565) # 设置图像色彩格式为RGB565格式
        sensor.set_framesize(sensor.QVGA)  # 设置图像大小为160*120
        sensor.set_auto_whitebal(False)      # 设置自动白平衡
        sensor.set_auto_gain(False)
        sensor.set_brightness(4000)         # 设置亮度为3000
        sensor.skip_frames(time = 40)       # 跳过帧
    elif a==2:
        sensor.reset()
        sensor.set_pixformat(sensor.RGB565) # 设置图像色彩格式为RGB565格式
        sensor.set_framesize(sensor.QQVGA)  # 设置图像大小为320*240
        sensor.set_auto_gain(False)
        sensor.set_auto_whitebal(True)      # 设置自动白平衡
        sensor.set_brightness(6000)         # 设置亮度为3000
        sensor.skip_frames(time = 20)       # 跳过帧
        sensor.set_auto_exposure(False,exposure_us=30000,gain_db=16)
conner_tranlate = [[0 for _ in range(2)] for _ in range(5)]
def deal_ConnerData(conner):
    global conner_tranlate

    short_len=5
    temp_i=0
    while (temp_i<4):
        if(temp_i<3):
            if conner[temp_i+1][0]==conner[temp_i][0] :
                angle_rad=math.pi/2
            else :
                temp_a=(conner[temp_i+1][1]-conner[temp_i][1])/(conner[temp_i+1][0]-conner[temp_i][0])
                angle_rad = math.atan(temp_a)  # 计算 arctan(1)，即反正切(1)
        else :
            if conner[0][0]==conner[temp_i][0] :
                angle_rad=math.pi/2
            else :
                temp_a=(conner[0][1]-conner[temp_i][1])/(conner[0][0]-conner[temp_i][0])
                angle_rad = math.atan(temp_a)
        actual_angle=angle_rad-math.pi/4
        if temp_i==4:
            conner_tranlate[temp_i][0]=conner[temp_i][0]+short_len*math.cos(actual_angle)
            conner_tranlate[temp_i][1]=conner[temp_i][1]-short_len*math.sin(actual_angle)
        elif temp_i ==3:
            conner_tranlate[temp_i][0]=conner[temp_i][0]-short_len*math.cos(actual_angle)
            conner_tranlate[temp_i][1]=conner[temp_i][1]-short_len*math.sin(actual_angle)
        elif temp_i==1:
            conner_tranlate[temp_i][0]=conner[temp_i][0]+short_len*math.cos(actual_angle)
            conner_tranlate[temp_i][1]=conner[temp_i][1]+short_len*math.sin(actual_angle)
        elif temp_i==2:
            conner_tranlate[temp_i][0]=conner[temp_i][0]-short_len*math.cos(actual_angle)
            conner_tranlate[temp_i][1]=conner[temp_i][1]-short_len*math.sin(actual_angle)
        else :
            conner_tranlate[temp_i][0]=conner[temp_i][0]+short_len*math.cos(actual_angle)
            conner_tranlate[temp_i][1]=conner[temp_i][1]+short_len*math.sin(actual_angle)
        temp_i+=1
        #print(angle_rad,temp_a,10*math.cos(actual_angle))

clock = time.clock()
cali_count=0
rec_black=0
rec_black_flag=0
temp_countb=0
count_pointb=0
start2=1
start_black=0
jd_k2=49
def run_black(coner):
    global jd_k2,start2,count_pointb,temp_countb,start_black,bx,by,tempx,tempy,x120,x40,y100,y20
    max_point=10
    conerx=[coner[0][0],coner[0][0],coner[0][0],coner[3][0],coner[3][0],coner[2][0],coner[2][0],coner[1][0],coner[1][0],coner[0][0]]
    conery=[coner[0][1],coner[0][1],coner[0][1],coner[3][1],coner[3][1],coner[2][1],coner[2][1],coner[1][1],coner[1][1],coner[0][1]]
    if mode!=7:

        if start2==1:
            if temp_countb==jd_k2:
                temp_countb=0
                count_pointb+=1
                if count_pointb == max_point:
                    start2=0
                    start_black=0
            else :
                temp_countb+=1
        if count_pointb ==max_point-1:
            tempx=bx-2*(conerx[count_pointb]+((conerx[0]-conerx[count_pointb])*temp_countb/jd_k2))
            tempy=by-2*(conery[count_pointb]+((conery[0]-conery[count_pointb])*temp_countb/jd_k2))
            #print (int(tempx),int (tempy))

        elif start_black ==1 :
            tempx=bx-2*(conerx[count_pointb]+((conerx[count_pointb+1]-conerx[count_pointb])*temp_countb/jd_k2))
            tempy=by-2*(conery[count_pointb]+((conery[count_pointb+1]-conery[count_pointb])*temp_countb/jd_k2))
            #print (tempx,tempy,conerx[count_pointb],conerx[0])

flag_cj=100
if __name__ == '__main__':
    sensor_init(1)
    max_corner=0
    while(True):
        if start1==0 and mode ==1:
            mode =0
        clock.tick()
        img = sensor.snapshot()
        if uart.any():
            cs=uart.readchar()
            if cs==48:        #0 复位
                mode=0
                rec_black=0
                start_black=0
                count_point=0
                temp_count=0
                rec_black_flag=0
            elif cs==49:      #1 走白框
                start =1
                mode=1
                count_point=0
            elif cs==50:      #2 走黑框
                mode=2
                if rec_black==0:
                    sensor_init(2)
                    rec_black=1
                else:
                    start_black=1
            elif cs==51:      #3 慢走黑框
                mode=2
                start_black=1
                jd_k2=142
            elif cs==55:      #7 暂停
                if mode_last==0:
                    mode_last = mode
                    mode=7
                    #tim.deinit()
                    #pyb.disable_irq()
                    print("enter stop")
                elif mode_last!=0:
                    mode=mode_last
                    mode_last=0
                    #pyb.disable_irq()
                    #tim = Timer(2, freq=25)      # 使用定时器2创建定时器对象-以1Hz触发
                    #tim.callback(tick)          # 将回调设置为tick函数
                    print("out stop")
            elif cs==57:      #9 走校正
                mode=-1
                cali_count=cali_count+1
                if cali_count==5:
                    cali_count=1
            print ("rec:",cs," mode:",mode,"co:",cali_count)

        if rec_black==1 and start_black==0:
            if rec_black_flag==0:
                for r in img.find_rects(roi=(0,0,160,120),threshold = 10000):
                    if r.w() > 17 and r.h() > 17:
                        #if (r.w()+r.h())>max_corner and flag_cj>40:
                            #max_corner=r.w()+r.h()
                            #corner_max=r.corners()
                            #print(corner_max)
                        img.draw_rectangle(r.rect(), color = (255, 0, 0), scale = 4)
                        corner = r.corners()
                        img.draw_circle(corner[0][0], corner[0][1], 5, color = (0, 0, 255), thickness = 2, fill = False)
                        img.draw_circle(corner[1][0], corner[1][1], 5, color = (0, 0, 255), thickness = 2, fill = False)
                        img.draw_circle(corner[2][0], corner[2][1], 5, color = (0, 0, 255), thickness = 2, fill = False)
                        img.draw_circle(corner[3][0], corner[3][1], 5, color = (0, 0, 255), thickness = 2, fill = False)
                        #start_black = 1

                        if flag_cj==0:
                            print ("corner:",corner)
                            deal_ConnerData(corner)
                            rec_black_flag=1
                            red_led.on()
                            pyb.delay(1000)
                            red_led.off()
                            sensor_init(1)
                            print ("translate:",conner_tranlate)
                        else :
                            flag_cj-=1
                        print (corner)
        elif mode==7:
            sending_data(0,0)

        else :
            red_blods =  img.find_blobs(red_td,pixels_threshold=2, area_threshold=4, merge=True,invert = 0)
            green_blods = img.find_blobs(green_td,pixels_threshold=2, area_threshold=4, merge=True,invert = 0)
            if red_blods or green_blods:
                if red_blods:
                    b = max(red_blods, key=lambda x: x.area())
                elif green_blods:
                    b = max(green_blods, key=lambda x: x.area())
                img.draw_circle((b.cx(), b.cy(),int((b.w()+b.h())/4)))
                    # 在屏幕上画出色块
                img.draw_rectangle(b.rect(), color = (0, 255, 0), scale = 2, thickness = 2)

                bx=(b.x() + b.w()/2)
                by=(b.y() + b.h()/2)
                if mode == 0:             #复位
                    sending_data(int (bx-163),int (by-124))
                elif mode == 1 :          #走白框
                    sending_data(int (tempx),int (tempy))
                elif mode == 2 :          #走黑框
                    if start_black != 1:
                        sending_data(int (bx-160),int (by-120))
                        print((bx-160))
                    else :
                        run_black(conner_tranlate)
                        sending_data(int (tempx),int (tempy))
                elif mode == -1 :         #校正
                    #print("cout",cali_count)
                    if cali_count == 1:
                        sending_data(int (bx-x40*2),int (by-y20*2))
                        print((bx-x40*2),(by-y20*2))
                    if cali_count == 2:
                        sending_data(int (bx-x120*2),int (by-y20*2))
                    if cali_count == 3:
                        sending_data(int (bx-x120*2),int (by-y100*2))
                    if cali_count == 4:
                        sending_data(int (bx-x40*2),int (by-y100*2))

                    sending_data(int (bx-160),int (by-120))

                #print( (b.x() + b.w()/2),(b.y() + b.h()/2))
        #img.draw_rectangle(roi1, color = (255, 0, 0), scale = 4)
        # 打印帧率
        #print(clock.fps())
